/*!
  @file vis_odometer.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/vis_odometer.h"

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <ros/ros.h>
#include <tf/transform_datatypes.h>

namespace vo {


void VisOdometer::compute(cv::Mat &relative_pose) {
  if (!initCompute()) {
    ROS_ERROR("Error in performing VO.");
    return;
  }

  if (relative_pose.type() != CV_32F && relative_pose.size() != cv::Size(4, 4)) {
    ROS_ERROR("Please initialize the pose matrix as cv::Mat(4, 4, CV_32F)");
    return;
  }

  FeaturePtr feature = getFeature();

  extractor_->setImages(img_vec_);
  extractor_->compute(feature);

  // this should be changed later ...
  if (frame_cnt_ == 1) {
    resetPose();
  } else if (frame_cnt_ > 1) {
    estimator_->setInputFeature(getFeature(-1));
    estimator_->setTargetFeature(getFeature(0));
    estimator_->compute(relative_pose);
    pose_ = pose_ * relative_pose;
  }
}

void VisOdometer::showImages(const std::vector<cv::Mat> &imgs, const std::string &id) {
  cv::Mat orig_disp_img;
  std::vector<cv::KeyPoint> null_kps;
  std::vector<cv::DMatch> null_matches;
  cv::drawMatches(imgs[0], null_kps, imgs[1], null_kps, null_matches, orig_disp_img);
  showSmallImage(orig_disp_img, id);
}

void VisOdometer::showKeyPoints() {
  std::vector<cv::Mat> tmp_imgs(2);
  for (size_t i = 0; i < 2; ++i) {
    cv::drawKeypoints(img_vec_[i], *getFeature()->kp_vec[i], tmp_imgs[i]);
  }
  showImages(tmp_imgs, "key points");
}

void VisOdometer::showTracks(bool show_inliers) {
  if (frame_cnt_ <= 1) return;
  cv::Mat orig_disp_img(img_vec_[0].size(), CV_8UC3);
  cv::cvtColor(img_vec_[0], orig_disp_img, CV_GRAY2BGR);
  boost::shared_ptr<Feature::KeyPoints> kp0 = getFeature(-1)->kp_vec[0];
  boost::shared_ptr<Feature::KeyPoints> kp1 = getFeature(0)->kp_vec[0];
  boost::shared_ptr<Feature::Matches> matches = estimator_->getMatches();

  cv::Scalar red(0, 20, 250);
  cv::Scalar green(60, 200, 0);
  if (show_inliers) {
    for (std::vector<int>::iterator ind_it = estimator_->getInlierIndices()->begin(); 
         ind_it != estimator_->getInlierIndices()->end(); ++ind_it) {
      cv::line(orig_disp_img, kp0->at(matches->at(*ind_it).queryIdx).pt, kp1->at(matches->at(*ind_it).trainIdx).pt, red, 2);
    }
  } else {
    for (Feature::Matches::iterator match_it = matches->begin();
        match_it != matches->end(); ++match_it) {
      cv::line(orig_disp_img, kp0->at(match_it->queryIdx).pt, kp1->at(match_it->trainIdx).pt, green, 2);
    }
  }
  //showSmallImage(orig_disp_img, "tracks");
  cv::imshow("tracks", orig_disp_img);
  cv::waitKey(1);
}

void VisOdometer::showMatches() {
  cv::Mat orig_disp_img;
  FeaturePtr feature = extractor_->getFeature();
  cv::drawMatches(img_vec_[0], *feature->kp_vec[0], img_vec_[1], *feature->kp_vec[1], *feature->matches, orig_disp_img);
  showSmallImage(orig_disp_img, "matches");
}

void VisOdometer::showSmallImage(const cv::Mat &orig_img, const std::string &winname) {
  cv::Size sz(orig_img.cols / 4, orig_img.rows / 4);
  cv::Mat disp_img(sz, CV_8U);
  cv::resize(orig_img, disp_img, sz);
  cv::imshow(winname, disp_img); 
  cv::waitKey(1);
}

void VisOdometer::publishTF() {
  tf::Transform transform;

  tf::Vector3 position(pose_.at<float>(0, 3), pose_.at<float>(1, 3), pose_.at<float>(2, 3));
  transform.setOrigin(position);

  tf::Matrix3x3 rotation;
  tf::Quaternion orientation;
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      rotation[i][j] = pose_.at<float>(i, j);
  rotation.getRotation(orientation);
  transform.setRotation(orientation);

  std::string cam_frame = "camera_frame";
  tf_pub_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", cam_frame));
}


}  // namespace vo
